#pragma once
#include "constants.h"
#include "tools.hpp"
#include <cmath>

class Node3D {
private:
    float x;
    float y;
    float t;
    float g;
    float h;
    int   idx;
    bool  o;
    bool  c;
    int   prim;

    const Node3D *pred;

public:
    Node3D() : Node3D( 0, 0, 0, 0, 0, nullptr ) {}

    Node3D( float x, float y, float t, float g, float h, const Node3D *pred, int prim = 0 ) {
        this->x    = x;
        this->y    = y;
        this->t    = t;
        this->g    = g;
        this->h    = h;
        this->pred = pred;
        this->o    = false;
        this->c    = false;
        this->idx  = -1;
        this->prim = prim;
    }

    float getX() const {
        return x;
    }
    float getY() const {
        return y;
    }
    float getT() const {
        return t;
    }
    float getG() const {
        return g;
    }
    float getH() const {
        return h;
    }
    float getC() const {
        return g + h;
    }
    int getIdx() const {
        return idx;
    }
    int getPrim() const {
        return prim;
    }
    bool isOpen() const {
        return o;
    }
    bool isClosed() const {
        return c;
    }
    const Node3D *getPred() const {
        return pred;
    }

    void setX( const float &x ) {
        this->x = x;
    }
    void setY( const float &y ) {
        this->y = y;
    }
    void setT( const float &t ) {
        this->t = t;
    }
    void setG( const float &g ) {
        this->g = g;
    }
    void setH( const float &h ) {
        this->h = h;
    }
    int setIdx( int width, int height ) {
        this->idx =
            ( int )( ( t + M_PI ) / Constants::deltaHeadingRad ) * width * height + ( int )( y )*width + ( int )( x );
        return idx;
    }
    void open() {
        o = true;
        c = false;
    }
    void close() {
        c = true;
        o = false;
    }
    void setPred( const Node3D *pred ) {
        this->pred = pred;
    }

    void updateG() {
        if ( pred->prim != prim ) {
            g += Constants::expand_step * Constants::penaltyTurning;
        } else {
            g += Constants::expand_step;
        }
    }

    bool isTheSameNode( const Node3D &rhs ) const {
        return ( x - rhs.x ) < 1.0 && ( y - rhs.y ) < 1.0
               && ( std::abs( t - rhs.t ) <= Constants::deltaHeadingRad
                    || std::abs( t - rhs.t ) >= Constants::deltaHeadingNegRad );
    }

    bool isInDubinRange( const Node3D &goal ) const {
        float dx   = std::abs( x - goal.x );
        float dy   = std::abs( y - goal.y );
        float dist = sqrt( ( dx * dx ) + ( dy * dy ) );
        return dist < Constants::dubinsShotDistance / Constants::cellSize;
    }

    bool isArrivedGoal( const Node3D &goal ) const {
        return fabs( x - goal.x ) <= Constants::dubinsStepSize && fabs( y - goal.y ) <= Constants::dubinsStepSize
               && ( fabs( t - goal.t ) <= Constants::deltaHeadingRad
                    || fabs( t - goal.t ) >= Constants::deltaHeadingNegRad );
    }

    bool isOnGrid( const int width, const int height ) const {
        return x >= 0 && x < width && y >= 0 && y < height && ( int )( ( t + M_PI ) / Constants::deltaHeadingRad ) >= 0
               && ( int )( ( t + M_PI ) / Constants::deltaHeadingRad ) < Constants::headings;
    }

    Node3D *createExpandNode( const int i, const int motion_type ) {
        double expand_x   = x + motion_type * Constants::dx[ i ] * cos( t ) - Constants::dy[ i ] * sin( t );
        double expand_y   = y + motion_type * Constants::dx[ i ] * sin( t ) + Constants::dy[ i ] * cos( t );
        double expand_yaw = Tools::normalize_angle( t + motion_type * Constants::dt[ i ] );

        Node3D *expand_node = new Node3D( expand_x, expand_y, expand_yaw, g, 0, this, i );

        return expand_node;
    }
};
